
#include <stdio.h>
#include "IGjkPairDetector.h"
#include "IVoronoiSimplexSolver.h"
#include "member_BShape.h"

IGjkPairDetector::IGjkPairDetector(BShape* shapeA, BShape* shapeB, IVoronoiSimplexSolver* simplexSolver)
{
	_separatingAxis.set(0, 1, 0);
	_simplexSolver = simplexSolver;
	_shapeA = shapeA;
	_shapeB = shapeB;
	_marginA = shapeA->margin();
	_marginB = shapeB->margin();
}

static void ComputeSupport(const BShape* shapeA, const BMatrix& localMatrixA, const BShape* shapeB, const BMatrix& localMatrixB, const BVector& dir, BVector& supAworld, BVector& supBworld, BVector& aMinb)
{
	BVector separatingAxisInA = localMatrixA.mult3(dir);
	BVector separatingAxisInB = localMatrixB.mult3(-dir);

	BVector pInANoMargin = shape_member(shapeA)->supportVertex(separatingAxisInA);
	BVector qInBNoMargin = shape_member(shapeB)->supportVertex(separatingAxisInB);

	BVector pInA = pInANoMargin;
	BVector qInB = qInBNoMargin;

	supAworld = localMatrixA * pInA;
	supBworld = localMatrixB * qInB;
	aMinb = supAworld - supBworld;
}

struct SupportVector
{
	BVector v;   //!< Support point in minkowski sum
	BVector v1;  //!< Support point in obj1
	BVector v2;  //!< Support point in obj2
};

struct SimplexInfo
{
	SupportVector ps[4];
	int last;
};

static BVector ccd_origin(0, 0, 0);

inline void SimplexInit(SimplexInfo *s)
{
	s->last = -1;
}

inline int SimplexSize(const SimplexInfo *s)
{
	return s->last + 1;
}

inline const SupportVector *SimplexPoint(const SimplexInfo *s, int idx)
{
	// here is no check on boundaries
	return &s->ps[idx];
}
inline void SupportCopy(SupportVector *d, const SupportVector *s)
{
	*d = *s;
}

inline void Vec3Copy(BVector *v, const BVector *w)
{
	*v = *w;
}

inline void ccdVec3Add(BVector *v, const BVector *w)
{
	v->x() += w->x();
	v->y() += w->y();
	v->z() += w->z();
}

inline void ccdVec3Sub(BVector *v, const BVector *w)
{
	*v -= *w;
}
inline void Vec3Sub2(BVector *d, const BVector *v, const BVector *w)
{
	*d = (*v) - (*w);
}
inline BReal Vec3Dot(const BVector *a, const BVector *b)
{
	BReal dot;
	dot = a->dot(*b);

	return dot;
}

inline BReal ccdVec3Dist2(const BVector *a, const BVector *b)
{
	BVector ab;
	Vec3Sub2(&ab, a, b);
	return Vec3Dot(&ab, &ab);
}

inline void Vec3Scale(BVector *d, BReal k)
{
	d->x() *= k;
	d->y() *= k;
	d->z() *= k;
}

inline void Vec3Cross(BVector *d, const BVector *a, const BVector *b)
{
	d->x() = (a->y() * b->z()) - (a->z() * b->y());
	d->y() = (a->z() * b->x()) - (a->x() * b->z());
	d->z() = (a->x() * b->y()) - (a->y() * b->x());
}

inline void TripleCross(const BVector* a, const BVector* b, const BVector* c, BVector* d)
{
	BVector e;
	Vec3Cross(&e, a, b);
	Vec3Cross(d, &e, c);
}

inline int ccdEq(BReal _a, BReal _b)
{
	BReal ab;
	BReal a, b;

	ab = fabs(_a - _b);
	if (fabs(ab) < REAL_EPSILON)
		return 1;

	a = fabs(_a);
	b = fabs(_b);
	if (b > a)
	{
		return ab < REAL_EPSILON * b;
	}
	else
	{
		return ab < REAL_EPSILON * a;
	}
}

BReal ccdVec3X(const BVector* v)
{
	return v->x();
}

BReal ccdVec3Y(const BVector* v)
{
	return v->y();
}

BReal ccdVec3Z(const BVector* v)
{
	return v->z();
}
inline int Vec3Eq(const BVector* a, const BVector* b)
{
	return ccdEq(ccdVec3X(a), ccdVec3X(b)) && ccdEq(ccdVec3Y(a), ccdVec3Y(b)) && ccdEq(ccdVec3Z(a), ccdVec3Z(b));
}

inline void SimplexAdd(SimplexInfo* s, const SupportVector* v)
{
	// here is no check on boundaries in sake of speed
	++s->last;
	SupportCopy(s->ps + s->last, v);
}

inline void SimplexSet(SimplexInfo* s, size_t pos, const SupportVector* a)
{
	SupportCopy(s->ps + pos, a);
}

inline void SimplexSetSize(SimplexInfo *s, int size)
{
	s->last = size - 1;
}

inline const SupportVector *ccdSimplexLast(const SimplexInfo *s)
{
	return SimplexPoint(s, s->last);
}

inline int ccdSign(BReal val)
{
	if (FuzzyZero(val))
	{
		return 0;
	}
	else if (val < BReal(0))
	{
		return -1;
	}
	return 1;
}

inline BReal Vec3PointSegmentDist2(const BVector* P, const BVector* x0, const BVector* b, BVector* witness)
{
	// The computation comes from solving equation of segment:
	//      S(t) = x0 + t.d
	//          where - x0 is initial point of segment
	//                - d is direction of segment from x0 (|d| > 0)
	//                - t belongs to <0, 1> interval
	//
	// Than, distance from a segment to some point P can be expressed:
	//      D(t) = |x0 + t.d - P|^2
	//          which is distance from any point on segment. Minimization
	//          of this function brings distance from P to segment.
	// Minimization of D(t) leads to simple quadratic equation that's
	// solving is straightforward.
	//
	// Bonus of this method is witness point for free.

	BReal dist, t;
	BVector d, a;

	// direction of segment
	Vec3Sub2(&d, b, x0);

	// precompute vector from P to x0
	Vec3Sub2(&a, x0, P);

	t = -BReal(1.) * Vec3Dot(&a, &d);
	t /= Vec3Dot(&d, &d);

	if (t < BReal(0) || FuzzyZero(t))
	{
		dist = ccdVec3Dist2(x0, P);
		if (witness)
			Vec3Copy(witness, x0);
	}
	else if (t > BReal(1) || ccdEq(t, BReal(1)))
	{
		dist = ccdVec3Dist2(b, P);
		if (witness)
			Vec3Copy(witness, b);
	}
	else
	{
		if (witness)
		{
			Vec3Copy(witness, &d);
			Vec3Scale(witness, t);
			ccdVec3Add(witness, x0);
			dist = ccdVec3Dist2(witness, P);
		}
		else
		{
			// recycling variables
			Vec3Scale(&d, t);
			ccdVec3Add(&d, &a);
			dist = Vec3Dot(&d, &d);
		}
	}

	return dist;
}

BReal Vec3PointTriDist2(const BVector* P, const BVector* x0, const BVector* B, const BVector* C, BVector* witness)
{
	// Computation comes from analytic expression for triangle (x0, B, C)
	//      T(s, t) = x0 + s.d1 + t.d2, where d1 = B - x0 and d2 = C - x0 and
	// Then equation for distance is:
	//      D(s, t) = | T(s, t) - P |^2
	// This leads to minimization of quadratic function of two variables.
	// The solution from is taken only if s is between 0 and 1, t is
	// between 0 and 1 and t + s < 1, otherwise distance from segment is
	// computed.

	BVector d1, d2, a;
	double u, v, w, p, q, r;
	double s, t, dist, dist2;
	BVector witness2;

	Vec3Sub2(&d1, B, x0);
	Vec3Sub2(&d2, C, x0);
	Vec3Sub2(&a, x0, P);

	u = Vec3Dot(&a, &a);
	v = Vec3Dot(&d1, &d1);
	w = Vec3Dot(&d2, &d2);
	p = Vec3Dot(&a, &d1);
	q = Vec3Dot(&a, &d2);
	r = Vec3Dot(&d1, &d2);

	s = (q * r - w * p) / (w * v - r * r);
	t = (-s * r - q) / w;

	if ((FuzzyZero(s) || s > BReal(0)) && (ccdEq(s, BReal(1)) || s < BReal(1)) &&
		(FuzzyZero(t) || t > BReal(0)) && (ccdEq(t, BReal(1)) || t < BReal(1)) &&
		(ccdEq(t + s, BReal(1)) || t + s < BReal(1)))
	{
		if (witness)
		{
			Vec3Scale(&d1, s);
			Vec3Scale(&d2, t);
			Vec3Copy(witness, x0);
			ccdVec3Add(witness, &d1);
			ccdVec3Add(witness, &d2);

			dist = ccdVec3Dist2(witness, P);
		}
		else
		{
			dist = s * s * v;
			dist += t * t * w;
			dist += BReal(2.) * s * t * r;
			dist += BReal(2.) * s * p;
			dist += BReal(2.) * t * q;
			dist += u;
		}
	}
	else
	{
		dist = Vec3PointSegmentDist2(P, x0, B, witness);

		dist2 = Vec3PointSegmentDist2(P, x0, C, &witness2);
		if (dist2 < dist)
		{
			dist = dist2;
			if (witness)
				Vec3Copy(witness, &witness2);
		}

		dist2 = Vec3PointSegmentDist2(P, B, C, &witness2);
		if (dist2 < dist)
		{
			dist = dist2;
			if (witness)
				Vec3Copy(witness, &witness2);
		}
	}

	return dist;
}

static int DoSimplex2(SimplexInfo* simplex, BVector* dir)
{
	const SupportVector* A, * B;
	BVector AB, AO, tmp;
	BReal dot;

	// get last added as A
	A = ccdSimplexLast(simplex);
	// get the other point
	B = SimplexPoint(simplex, 0);
	// compute AB oriented segment
	Vec3Sub2(&AB, &B->v, &A->v);
	// compute AO vector
	Vec3Copy(&AO, &A->v);
	Vec3Scale(&AO, -BReal(1));

	// dot product AB . AO
	dot = Vec3Dot(&AB, &AO);

	// check if origin doesn't lie on AB segment
	Vec3Cross(&tmp, &AB, &AO);
	if (FuzzyZero(Vec3Dot(&tmp, &tmp)) && dot > BReal(0))
	{
		return 1;
	}

	// check if origin is in area where AB segment is
	if (FuzzyZero(dot) || dot < BReal(0))
	{
		// origin is in outside are of A
		SimplexSet(simplex, 0, A);
		SimplexSetSize(simplex, 1);
		Vec3Copy(dir, &AO);
	}
	else
	{
		// origin is in area where AB segment is

		// keep simplex untouched and set direction to
		// AB x AO x AB
		TripleCross(&AB, &AO, &AB, dir);
	}

	return 0;
}

static int DoSimplex3(SimplexInfo* simplex, BVector* dir)
{
	const SupportVector* A, * B, * C;
	BVector AO, AB, AC, ABC, tmp;
	BReal dot, dist;

	// get last added as A
	A = ccdSimplexLast(simplex);
	// get the other points
	B = SimplexPoint(simplex, 1);
	C = SimplexPoint(simplex, 0);

	// check touching contact
	dist = Vec3PointTriDist2(&ccd_origin, &A->v, &B->v, &C->v, 0);
	if (FuzzyZero(dist))
	{
		return 1;
	}

	// check if triangle is really triangle (has area > 0)
	// if not simplex can't be expanded and thus no itersection is found
	if (Vec3Eq(&A->v, &B->v) || Vec3Eq(&A->v, &C->v))
	{
		return -1;
	}

	// compute AO vector
	Vec3Copy(&AO, &A->v);
	Vec3Scale(&AO, -BReal(1));

	// compute AB and AC segments and ABC vector (perpendircular to triangle)
	Vec3Sub2(&AB, &B->v, &A->v);
	Vec3Sub2(&AC, &C->v, &A->v);
	Vec3Cross(&ABC, &AB, &AC);

	Vec3Cross(&tmp, &ABC, &AC);
	dot = Vec3Dot(&tmp, &AO);
	if (FuzzyZero(dot) || dot > BReal(0))
	{
		dot = Vec3Dot(&AC, &AO);
		if (FuzzyZero(dot) || dot > BReal(0))
		{
			// C is already in place
			SimplexSet(simplex, 1, A);
			SimplexSetSize(simplex, 2);
			TripleCross(&AC, &AO, &AC, dir);
		}
		else
		{
			dot = Vec3Dot(&AB, &AO);
			if (FuzzyZero(dot) || dot > BReal(0))
			{
				SimplexSet(simplex, 0, B);
				SimplexSet(simplex, 1, A);
				SimplexSetSize(simplex, 2);
				TripleCross(&AB, &AO, &AB, dir);
			}
			else
			{
				SimplexSet(simplex, 0, A);
				SimplexSetSize(simplex, 1);
				Vec3Copy(dir, &AO);
			}
		}
	}
	else
	{
		Vec3Cross(&tmp, &AB, &ABC);
		dot = Vec3Dot(&tmp, &AO);
		if (FuzzyZero(dot) || dot > BReal(0))
		{
			dot = Vec3Dot(&AB, &AO);
			if (FuzzyZero(dot) || dot > BReal(0))
			{
				SimplexSet(simplex, 0, B);
				SimplexSet(simplex, 1, A);
				SimplexSetSize(simplex, 2);
				TripleCross(&AB, &AO, &AB, dir);
			}
			else
			{
				SimplexSet(simplex, 0, A);
				SimplexSetSize(simplex, 1);
				Vec3Copy(dir, &AO);
			}
		}
		else
		{
			dot = Vec3Dot(&ABC, &AO);
			if (FuzzyZero(dot) || dot > BReal(0))
			{
				Vec3Copy(dir, &ABC);
			}
			else
			{
				SupportVector tmp;
				SupportCopy(&tmp, C);
				SimplexSet(simplex, 0, B);
				SimplexSet(simplex, 1, &tmp);

				Vec3Copy(dir, &ABC);
				Vec3Scale(dir, -BReal(1));
			}
		}
	}

	return 0;
}

static int DoSimplex4(SimplexInfo* simplex, BVector* dir)
{
	const SupportVector* A, * B, * C, * D;
	BVector AO, AB, AC, AD, ABC, ACD, ADB;
	int B_on_ACD, C_on_ADB, D_on_ABC;
	int AB_O, AC_O, AD_O;
	BReal dist;

	// get last added as A
	A = ccdSimplexLast(simplex);
	// get the other points
	B = SimplexPoint(simplex, 2);
	C = SimplexPoint(simplex, 1);
	D = SimplexPoint(simplex, 0);

	// check if tetrahedron is really tetrahedron (has volume > 0)
	// if it is not simplex can't be expanded and thus no intersection is
	// found
	dist = Vec3PointTriDist2(&A->v, &B->v, &C->v, &D->v, 0);
	if (FuzzyZero(dist))
	{
		return -1;
	}

	// check if origin lies on some of tetrahedron's face - if so objects
	// intersect
	dist = Vec3PointTriDist2(&ccd_origin, &A->v, &B->v, &C->v, 0);
	if (FuzzyZero(dist))
		return 1;
	dist = Vec3PointTriDist2(&ccd_origin, &A->v, &C->v, &D->v, 0);
	if (FuzzyZero(dist))
		return 1;
	dist = Vec3PointTriDist2(&ccd_origin, &A->v, &B->v, &D->v, 0);
	if (FuzzyZero(dist))
		return 1;
	dist = Vec3PointTriDist2(&ccd_origin, &B->v, &C->v, &D->v, 0);
	if (FuzzyZero(dist))
		return 1;

	// compute AO, AB, AC, AD segments and ABC, ACD, ADB normal vectors
	Vec3Copy(&AO, &A->v);
	Vec3Scale(&AO, -BReal(1));
	Vec3Sub2(&AB, &B->v, &A->v);
	Vec3Sub2(&AC, &C->v, &A->v);
	Vec3Sub2(&AD, &D->v, &A->v);
	Vec3Cross(&ABC, &AB, &AC);
	Vec3Cross(&ACD, &AC, &AD);
	Vec3Cross(&ADB, &AD, &AB);

	// side (positive or negative) of B, C, D relative to planes ACD, ADB
	// and ABC respectively
	B_on_ACD = ccdSign(Vec3Dot(&ACD, &AB));
	C_on_ADB = ccdSign(Vec3Dot(&ADB, &AC));
	D_on_ABC = ccdSign(Vec3Dot(&ABC, &AD));

	// whether origin is on same side of ACD, ADB, ABC as B, C, D
	// respectively
	AB_O = ccdSign(Vec3Dot(&ACD, &AO)) == B_on_ACD;
	AC_O = ccdSign(Vec3Dot(&ADB, &AO)) == C_on_ADB;
	AD_O = ccdSign(Vec3Dot(&ABC, &AO)) == D_on_ABC;

	if (AB_O && AC_O && AD_O)
	{
		// origin is in tetrahedron
		return 1;
		// rearrange simplex to triangle and call DoSimplex3()
	}
	else if (!AB_O)
	{
		// B is farthest from the origin among all of the tetrahedron's
		// points, so remove it from the list and go on with the triangle
		// case

		// D and C are in place
		SimplexSet(simplex, 2, A);
		SimplexSetSize(simplex, 3);
	}
	else if (!AC_O)
	{
		// C is farthest
		SimplexSet(simplex, 1, D);
		SimplexSet(simplex, 0, B);
		SimplexSet(simplex, 2, A);
		SimplexSetSize(simplex, 3);
	}
	else
	{  // (!AD_O)
		SimplexSet(simplex, 0, C);
		SimplexSet(simplex, 1, B);
		SimplexSet(simplex, 2, A);
		SimplexSetSize(simplex, 3);
	}

	return DoSimplex3(simplex, dir);
}

static int DoSimplex(SimplexInfo* simplex, BVector* dir)
{
	if (SimplexSize(simplex) == 2)
	{
		// simplex contains segment only one segment
		return DoSimplex2(simplex, dir);
	}
	else if (SimplexSize(simplex) == 3)
	{
		// simplex contains triangle
		return DoSimplex3(simplex, dir);
	}
	else
	{  // SimplexSize(simplex) == 4
		// tetrahedron - this is the only shape which can encapsule origin
		// so DoSimplex4() also contains test on it
		return DoSimplex4(simplex, dir);
	}
}

void IGjkPairDetector::closestPoints(const BMatrix& matrixA, const BMatrix& matrixB, IDetectorResult& result)
{
	BMatrix localMatrixA = matrixA;
	BMatrix localMatrixB = matrixB;
	BVector center = (localMatrixA.position() + localMatrixB.position()) * BReal(0.5);
	localMatrixA.setPosition(localMatrixA.position() - center);
	localMatrixB.setPosition(localMatrixB.position() - center);

	BReal marginA = _marginA;
	BReal marginB = _marginB;
	BReal margin = marginA + marginB;

	bool isValid = false;
	bool checkSimplex = false;
	bool checkPenetration = true;

	BVector orgNormalB;
	BVector normalB;
	BVector pointA;
	BVector pointB;
	BReal distance = 0;

	BReal squaredDistance = REAL_LARGE;
	BReal delta = 0;

	SimplexInfo simplex1;
	SimplexInfo* simplex = &simplex1;
	SimplexInit(simplex);

	BVector dir(1, 0, 0);

	BVector lastSupV;
	BVector supAworld;
	BVector supBworld;
	ComputeSupport(_shapeA, localMatrixA, _shapeB, localMatrixB, dir, supAworld, supBworld, lastSupV);

	SupportVector last;
	last.v = lastSupV;
	last.v1 = supAworld;
	last.v2 = supBworld;

	SimplexAdd(simplex, &last);

	dir = -lastSupV;

	int status = -2;
	for (int iterations = 0; iterations < 1000; iterations++)
	{
		// obtain support point
		ComputeSupport(_shapeA, localMatrixA, _shapeB, localMatrixB, dir, supAworld, supBworld, lastSupV);

		// check if farthest point in Minkowski difference in direction dir
		// isn't somewhere before origin (the test on negative dot product)
		// - because if it is, objects are not intersecting at all.
		BReal delta = lastSupV.dot(dir);
		if (delta < 0)
		{
			//no intersection, besides margin
			status = -1;
			break;
		}

		// add last support vector to simplex
		last.v = lastSupV;
		last.v1 = supAworld;
		last.v2 = supBworld;

		SimplexAdd(simplex, &last);

		// if DoSimplex returns 1 if objects intersect, -1 if objects don't
		// intersect and 0 if algorithm should continue

		BVector newDir;
		int do_simplex_res = DoSimplex(simplex, &dir);

		if (do_simplex_res == 1)
		{
			status = 0;  // intersection found
			break;
		}
		else if (do_simplex_res == -1)
		{
			// intersection not found
			status = -1;
			break;
		}

		if (FuzzyZero(Vec3Dot(&dir, &dir)))
		{
			// intersection not found
			status = -1;
		}

		if (dir.length2() < REAL_EPSILON)
		{
			//no intersection, besides margin
			status = -1;
			break;
		}

		if (FuzzyZero(dir))
		{
			// intersection not found
			status = -1;
			break;
		}
	}

	if (status == 0)
	{
		//printf("Intersect!\n");
	}
	if (status == -1)
	{
		//printf("not intersect\n");
	}
	//printf("dir=%f,%f,%f\n",dir[0],dir[1],dir[2]);

	_simplexSolver->reset();
	_separatingAxis.set(0, 1, 0);
	int curIter = 0;
	while (curIter++ < 1000)
	{
		BVector separatingAxisInA = localMatrixA.inverse().mult3(-_separatingAxis);
		BVector separatingAxisInB = localMatrixB.inverse().mult3(_separatingAxis);

		BVector pInA = shape_member(_shapeA)->supportVertex(separatingAxisInA);
		BVector qInB = shape_member(_shapeB)->supportVertex(separatingAxisInB);

		BVector pWorld = localMatrixA * pInA;
		BVector qWorld = localMatrixB * qInB;

		BVector w = pWorld - qWorld;
		delta = _separatingAxis.dot(w);
		if ((delta > 0) && (delta * delta > squaredDistance * REAL_MAX))
		{
			checkSimplex = true;
			break;
		}

		if (_simplexSolver->inSimplex(w))
		{
			checkSimplex = true;
			break;
		}

		BReal f0 = squaredDistance - delta;
		BReal f1 = squaredDistance * REAL_EPSILON;
		if (f0 <= f1)
		{
			checkSimplex = true;
			break;
		}

		_simplexSolver->addVertex(w, pWorld, qWorld);
		BVector newCachedSeparatingAxis;
		if (!_simplexSolver->closest(newCachedSeparatingAxis))
		{
			checkSimplex = true;
			break;
		}

		if (newCachedSeparatingAxis.length2() < REAL_EPSILON)
		{
			_separatingAxis = newCachedSeparatingAxis;
			checkSimplex = true;
			break;
		}

		BReal previousSquaredDistance = squaredDistance;
		squaredDistance = newCachedSeparatingAxis.length2();
		if (previousSquaredDistance - squaredDistance <= REAL_EPSILON * previousSquaredDistance)
		{
			checkSimplex = true;
			break;
		}

		_separatingAxis = newCachedSeparatingAxis;

		if (_simplexSolver->fullSimplex())
		{
			break;
		}
	}

	if (checkSimplex)
	{
		_simplexSolver->compute_points(pointA, pointB);
		normalB = _separatingAxis;

		BReal lenSqr = _separatingAxis.length2();
		if (lenSqr > REAL_EPSILON)
		{
			BReal rlen = sqrt(lenSqr);
			normalB /= rlen;

			BReal s = sqrt(squaredDistance);

			pointA -= _separatingAxis * (marginA / s);
			pointB += _separatingAxis * (marginB / s);
			distance = rlen - margin;
			isValid = true;
			orgNormalB = normalB;
		}
	}

	bool catchDegeneratePenetrationCase = checkSimplex && (distance + margin < REAL_EPSILON);
	if ((checkPenetration && (!isValid || catchDegeneratePenetrationCase)) || (status == 0))
	{
		_separatingAxis.set(0, 0, 0);
		IGjkEpaSolver::Result results;
		bool isValid2 = this->calcPenDepth(_shapeA, _shapeB, localMatrixA, localMatrixB, results);
		if (results.normal.length2() > REAL_EPSILON)
		{
			_separatingAxis = results.normal;
			if (isValid2)
			{
				BVector tmpNormalInB = results.pointB - results.pointA;
				BReal lenSqr = tmpNormalInB.length2();
				if (lenSqr <= REAL_EPSILON)
				{
					tmpNormalInB = _separatingAxis;
					lenSqr = _separatingAxis.length2();
				}

				if (lenSqr > REAL_EPSILON)
				{
					tmpNormalInB /= sqrt(lenSqr);
					BReal dist = -(results.pointA - results.pointB).length();
					if (!isValid || (dist < distance))
					{
						distance = dist;
						pointA = results.pointA;
						pointB = results.pointB;
						normalB = tmpNormalInB;
						isValid = true;
					}
				}
			}
			else
			{
				BReal dist = (results.pointA - results.pointB).length() - margin;
				if (!isValid || (dist < distance))
				{
					distance = dist;
					pointA = results.pointA;
					pointB = results.pointB;
					pointA -= _separatingAxis * marginA;
					pointB += _separatingAxis * marginB;
					normalB = _separatingAxis;
					normalB.normalize();
					isValid = true;
				}
			}
		}
	}

	if (isValid && distance < 0)
	{
		_separatingAxis = normalB;
		///todo: need to track down this EPA penetration solver degeneracy
		///the penetration solver reports penetration but the contact normal
		///connecting the contact points is pointing in the opposite direction
		///until then, detect the issue and revert the normal

		BReal d2 = 0;
		{
			BVector separatingAxisInA = localMatrixA.inverse().mult3(-orgNormalB);
			BVector separatingAxisInB = localMatrixB.inverse().mult3(orgNormalB);

			BVector pInA = shape_member(_shapeA)->supportVertex(separatingAxisInA);
			BVector qInB = shape_member(_shapeB)->supportVertex(separatingAxisInB);

			BVector pWorld = localMatrixA * pInA;
			BVector qWorld = localMatrixB * qInB;
			BVector w = pWorld - qWorld;
			d2 = orgNormalB.dot(w) - margin;
		}

		BReal d1 = 0;
		{
			BVector separatingAxisInA = localMatrixA.inverse().mult3(normalB);
			BVector separatingAxisInB = localMatrixB.inverse().mult3(-normalB);

			BVector pInA = shape_member(_shapeA)->supportVertex(separatingAxisInA);
			BVector qInB = shape_member(_shapeB)->supportVertex(separatingAxisInB);

			BVector pWorld = localMatrixA * pInA;
			BVector qWorld = localMatrixB * qInB;
			BVector w = pWorld - qWorld;
			d1 = (-normalB).dot(w) - margin;
		}

		BReal d0 = 0;
		{
			BVector separatingAxisInA = localMatrixA.inverse().mult3(-normalB);
			BVector separatingAxisInB = localMatrixB.inverse().mult3(normalB);

			BVector pInA = shape_member(_shapeA)->supportVertex(separatingAxisInA);
			BVector qInB = shape_member(_shapeB)->supportVertex(separatingAxisInB);

			BVector pWorld = localMatrixA * pInA;
			BVector qWorld = localMatrixB * qInB;
			BVector w = pWorld - qWorld;
			d0 = normalB.dot(w) - margin;
		}

		if (d1 > d0)
		{
			normalB *= -1;
		}

		if (orgNormalB.length2())
		{
			if (d2 > d0 && d2 > d1 && d2 > distance)
			{
				normalB = orgNormalB;
				distance = d2;
			}
		}

		result.addContactPoint(pointB + center, normalB, distance);
	}
}

bool IGjkPairDetector::calcPenDepth(BShape* shapeA, BShape* shapeB, const BMatrix& matrixA, const BMatrix& matrixB, IGjkEpaSolver::Result& result)
{
	BVector vector = (matrixB.position() - matrixA.position()).normal();
	BVector guessVectors[] = { vector, -vector,
		BVector(0, 0, 1),
		BVector(0, 1, 0),
		BVector(1, 0, 0),
		BVector(1, 1, 0),
		BVector(0, 1, 1),
		BVector(1, 0, 1),
		BVector(1, 1, 1),
	};

	for (int i = 0; i < 9; i++)
	{
		const BVector& direct = guessVectors[i];
		if (IGjkEpaSolver::Penetration(shapeA, matrixA, shapeB, matrixB, direct, result))
		{
			return true;
		}
		else
		{
			if (IGjkEpaSolver::Distance(shapeA, matrixA, shapeB, matrixB, direct, result))
			{
				return false;
			}
		}
	}

	return false;
}
